/**
 * @file robot_node.cpp
 * @brief 机器人节点，实现ROS与小车底盘通信
 * @author Linfu Wei (ghowoght@qq.com)
 * @version 1.0
 * @date 2021-07-06
 * 
 * @copyright Copyright (c) 2021  WHU-EIS
 * 
 */
#include <iostream>
#include <string>
#include <sstream>

#include <ros/ros.h>
#include <std_msgs/String.h>
#include <serial/serial.h>

#include <nav_msgs/Odometry.h>  
#include <tf/transform_broadcaster.h>

const double R2D = 180.0 / 3.1415926535;
const double D2R = 3.1415926535 / 180.0;

/**
 * @brief 数据类型转换联合体，用于float和字节数组间的转换
 */
typedef union
{
	float data;
	unsigned char data8[4];
} data_u;

int main(int argc, char** argv)
{
	ros::init(argc, argv, "serial_node");
	ros::NodeHandle nh;
	ros::NodeHandle private_nh("~");

	serial::Serial ser;
	std::string serial_port;
	int serial_baudrate;
	private_nh.param("serial_port", serial_port, std::string("/dev/ttyACM0"));
	private_nh.param("serial_baudrate", serial_baudrate, 115200);
	// 打开串口
	try
	{
		ser.setPort(serial_port);
		ser.setBaudrate(serial_baudrate);
		serial::Timeout to = serial::Timeout::simpleTimeout(1000);
		ser.setTimeout(to);
		ser.open();
	}
	catch(serial::IOException &e)
	{
		ROS_INFO_STREAM("Failed to open port ");
		return -1;
	}
	ROS_INFO_STREAM("Succeed to open port" );

	// 清空串口缓存区
	ser.read(ser.available());

	// 底盘复位
	uint8_t _cnt = 0;
	uint8_t data_to_send[100]; // 待发送的字节数组
	data_to_send[_cnt++]=0xAA;
	data_to_send[_cnt++]=0x55;
	uint8_t _start = _cnt;
	data_to_send[_cnt++]=0x11;
	data_to_send[_cnt++]=0x22;
	data_to_send[_cnt++]=0x33;
	data_to_send[_cnt++]=0x44;
	data_to_send[_cnt++]=0x55;
	data_to_send[_cnt++]=0x66;
	data_to_send[_cnt++]=0x77;
	data_to_send[_cnt++]=0x88;
	data_to_send[_cnt++]=0x99;
	data_to_send[_cnt++]=0xAA;
	data_to_send[_cnt++]=0xBB;
	data_to_send[_cnt++]=0xCC;
    char checkout = 0;
    for(int i = _start; i < _cnt; i++)
    {
        checkout += data_to_send[i];
    }
	data_to_send[_cnt++] = checkout;

    // 串口发送
    ser.write(data_to_send, _cnt);
    // 等待底盘复位
	ros::Duration(1.0).sleep();

    // 里程计话题
	ros::Publisher odom_pub = nh.advertise<nav_msgs::Odometry>("odom", 50);
	tf::TransformBroadcaster odom_broadcaster;
	ros::Subscriber sub = nh.subscribe<geometry_msgs::Twist>("/cmd_vel", 10, [&](const geometry_msgs::Twist::ConstPtr& cmd_vel){
		uint8_t _cnt = 0;
		data_u _temp; // 声明一个联合体实例，使用它将待发送数据转换为字节数组
		uint8_t data_to_send[100]; // 待发送的字节数组		
		data_to_send[_cnt++]=0xAA;
		data_to_send[_cnt++]=0x55;
		uint8_t _start = _cnt;
		float datas[] = {	(float)cmd_vel->linear.x,
							(float)cmd_vel->linear.y,
							(float)cmd_vel->angular.z};		
		for(int i = 0; i < sizeof(datas) / sizeof(float); i++){
			// 将要发送的数据赋值给联合体的float成员
			// 相应的就能更改字节数组成员的值
			_temp.data = datas[i];
			data_to_send[_cnt++]=_temp.data8[0];
			data_to_send[_cnt++]=_temp.data8[1];
			data_to_send[_cnt++]=_temp.data8[2];
			data_to_send[_cnt++]=_temp.data8[3]; // 最高位
		}
		// 添加校验码
		char checkout = 0;
		for(int i = _start; i < _cnt; i++)
			checkout += data_to_send[i];
		data_to_send[_cnt++] = checkout;
		// 串口发送
		ser.write(data_to_send, _cnt);
	});

	nav_msgs::Odometry odom;
	geometry_msgs::TransformStamped odom_trans;

	float x 	= 0.0;	// x轴位置
	float y 	= 0.0;	// y轴位置

	ros::Time current_time, last_time;
	current_time = ros::Time::now();
 	last_time = ros::Time::now();

	// 设置读取频率，读取频率 > 数据包长度 * 上传频率
	ros::Rate loop_rate(500); 

	int state = 0;
	uint8_t r_buffer[1024] = {0};
	unsigned char data_receive[16] = {0};
    int data_cnt = 0;
    while(ros::ok())
	{
		if(ser.available())
		{
            uint8_t buffer = 0;
			ser.read(&buffer, 1);

			if(state == 0 && buffer == 0xAA)
			{
				state++;
			}
			else if(state == 1 && buffer == 0x55)
			{
				state++;
			}
			else if(state == 2)
			{
				data_receive[data_cnt++]=buffer;
				if(data_cnt == 17)
				{
					/* 进行数据校验 */
					uint8_t checkout = 0;
					for(int k = 0; k < data_cnt - 1; k++)
					{
						checkout += data_receive[k];
					}
					if(checkout == data_receive[data_cnt - 1]) // 串口接收到的最后一个字节是校验码 
					{
						/* 校验通过，进行解码 */
						float vx, vy, vth, th; // x轴线速度，y轴线速度，z轴角速度，偏航角
						float* datas_ptr[] = {&vx, &vy, &vth, &th};
						data_u temp;
						for(int i = 0; i < sizeof(datas_ptr) / sizeof(float*); i++)
						{
							temp.data8[0] = data_receive[4 * i + 0];
							temp.data8[1] = data_receive[4 * i + 1];
							temp.data8[2] = data_receive[4 * i + 2];
							temp.data8[3] = data_receive[4 * i + 3];							
							*(datas_ptr[i]) = temp.data;
						}
						vy = -vy;
						th *= D2R; // 转换为弧度
  
						/* 对速度进行积分得到位移 */
						// 获取当前时间
						current_time = ros::Time::now();
						// 获取积分间隔
						double dt = (current_time - last_time).toSec();
						last_time = current_time;
						// 将机体系速度转换到里程计坐标系
						double delta_x = (vx * cos(th) - vy * sin(th)) * dt;
						double delta_y = (vx * sin(th) + vy * cos(th)) * dt;
						// 速度积分
						x += delta_x;
						y += delta_y;

						// 将偏航角从欧拉角形式转换为四元数形式
                        // 在机器人学中，不是用角度形式表示机器人姿态，而是同过四元数/旋转矩阵的形式来表示
						geometry_msgs::Quaternion odom_quat = tf::createQuaternionMsgFromYaw(th);
						
						odom_trans.header.stamp = current_time;
						odom_trans.header.frame_id = "odom";
						odom_trans.child_frame_id = "base_link";

						odom_trans.transform.translation.x = x;
						odom_trans.transform.translation.y = y;
						odom_trans.transform.translation.z = 0.0;
						odom_trans.transform.rotation = odom_quat;
						// 发布坐标变换
						odom_broadcaster.sendTransform(odom_trans);

						odom.header.stamp = current_time;
						odom.header.frame_id = "odom";

						// 设置机器人的位置和姿态
						odom.pose.pose.position.x = x;
						odom.pose.pose.position.y = y;
						odom.pose.pose.position.z = 0.0;
						odom.pose.pose.orientation = odom_quat;

						// 设置机器人的速度
						odom.child_frame_id = "base_link";
						odom.twist.twist.linear.x = vx;
						odom.twist.twist.linear.y = vy;
						odom.twist.twist.angular.z = vth;

						// 发布里程计消息
						odom_pub.publish(odom);

						ROS_INFO("linear_x: %f, linear_y: %f, angular_z: %f", x, y, th * R2D);
					}					
					
					data_cnt = 0;
					state = 0;
				}
			}
			else state = 0;
		}

		ros::spinOnce();
		loop_rate.sleep();
	}
	// 关闭串口
	ser.close();

	return 0;
}